using BepuUtilities;
using BepuUtilities.Memory;
using System;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Constraints
{
    /// <summary>
    /// 约束连接三个实体中心的三角形的区域以匹配目标区域。
    /// 根据||ab x ac||计算的缩放体积。
    /// </summary>
    public struct AreaConstraint : IThreeBodyConstraintDescription<AreaConstraint>
    {
        /// <summary>
        /// 是三角形目标面积的2倍。从||ab x ac||计算得出。
        /// </summary>
        public float TargetScaledArea;
        /// <summary>
        /// 弹簧频率和阻尼参数。
        /// </summary>
        public SpringSettings SpringSettings;

        /// <summary>
        /// 创建新的区域约束,使用一组初始位置初始化目标区域。
        /// </summary>
        /// <param name="a">第一个正文的初始位置。</param>
        /// <param name="b">第二个正文的初始位置。</param>
        /// <param name="c">第三个主体的初始位置。</param>
        /// <param name="springSettings">要应用于体积约束的Spring设置。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public AreaConstraint(in Vector3 a, in Vector3 b, in Vector3 c, SpringSettings springSettings)
        {
            TargetScaledArea = Vector3.Cross(b - a, c - a).Length();
            SpringSettings = springSettings;
        }

        public int ConstraintTypeId
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return AreaConstraintTypeProcessor.BatchTypeId;
            }
        }

        public Type TypeProcessorType => typeof(AreaConstraintTypeProcessor);

        public void ApplyDescription(ref TypeBatch batch, int bundleIndex, int innerIndex)
        {
            Debug.Assert(TargetScaledArea >= 0, "AreaConstraint.TargetScaledArea must be nonnegative.");
            ConstraintChecker.AssertValid(SpringSettings, nameof(AreaConstraint));
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var target = ref GetOffsetInstance(ref Buffer<AreaConstraintPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            Unsafe.As<Vector<float>, float>(ref target.TargetScaledArea) = TargetScaledArea;
            SpringSettingsWide.WriteFirst(SpringSettings, ref target.SpringSettings);
        }

        public void BuildDescription(ref TypeBatch batch, int bundleIndex, int innerIndex, out AreaConstraint description)
        {
            Debug.Assert(ConstraintTypeId == batch.TypeId, "The type batch passed to the description must match the description's expected type.");
            ref var source = ref GetOffsetInstance(ref Buffer<AreaConstraintPrestepData>.Get(ref batch.PrestepData, bundleIndex), innerIndex);
            description.TargetScaledArea = Unsafe.As<Vector<float>, float>(ref source.TargetScaledArea);
            SpringSettingsWide.ReadFirst(source.SpringSettings, out description.SpringSettings);
        }
    }

    public struct AreaConstraintPrestepData
    {
        public Vector<float> TargetScaledArea;
        public SpringSettingsWide SpringSettings;
    }

    public struct AreaConstraintProjection
    {
        public Vector3Wide JacobianB;
        public Vector3Wide JacobianC;
        public Vector<float> EffectiveMass;
        public Vector<float> BiasImpulse;
        public Vector<float> SoftnessImpulseScale;
        public Vector<float> InverseMassA;
        public Vector<float> InverseMassB;
        public Vector<float> InverseMassC;
    }

    public struct AreaConstraintFunctions : IThreeBodyConstraintFunctions<AreaConstraintPrestepData, AreaConstraintProjection, Vector<float>>
    {
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Prestep(Bodies bodies, ref ThreeBodyReferences bodyReferences, int count, float dt, float inverseDt,
            ref BodyInertias inertiaA, ref BodyInertias inertiaB, ref BodyInertias inertiaC,
            ref AreaConstraintPrestepData prestep, out AreaConstraintProjection projection)
        {
            bodies.GatherOffsets(ref bodyReferences, count, out var ab, out var ac);

            // 具有顶点a、b和c的三角形的面积为：
            // |ab x ac||*0.5
            // 因此,约束条件是：
            // OriginalArea*2=||ab x ac||
            // 导致速度约束：
            // d/dt(OriginalArea*2)=d/dt(||ab x ac||)
            // 0=d/dt(点(ab x ac,ab x ac)^0.5)
            // 0=d/dt(点(ab x ac,ab x ac))*0.5*点(ab x ac,ab x ac)^-0.5)
            // 0=(点(d/dt(Ab X Ac),ab x ac)+dot(ab x ac,d/dt(Ab X Ac)*0.5*点(ab x ac,ab x ac)^-0.5)
            // 0=(2*点(d/dt(Ab X Ac),ab x ac))*0.5*点(ab x ac,ab x ac)^-0.5
            // 0=(2*点(d/dt(Ab)x ac+ab x d/dt(Ac),ab x ac))*0.5*点(ab x ac,ab x ac)^-0.5)
            // 0=点(d/dt(Ab)x ac+ab x d/dt(Ac),ab x ac)*点(ab x ac,ab x ac)^-0.5
            // 0=点(d/dt(Ab)x ac+ab x d/dt(Ac),ab x ac)/||ab x ac||
            // 0=(dot(d/dt(Ab)x ac,ab x ac)+dot(ab x d/dt(Ac),ab x ac))/||ab x ac||
            // 0=(dot(ac x(Ab X Ac),d/dt(Ab))+dot((Ab X Ac)x ab,d/dt(Ac)/||ab x ac||
            // 0=点(ac x((Ab X Ac)/||ab x ac||),d/dt(Ab))+点(Ab X Ac)/||ab x ac||)x ab,d/dt(Ac))
            Vector3Wide.CrossWithoutOverlap(ab, ac, out var abxac);
            Vector3Wide.Length(abxac, out var normalLength);
            // 如果边平行或反平行,则三角形法线长度可以为零。对潜在的除法进行零保护。
            Vector3Wide.Scale(abxac, Vector.ConditionalSelect(Vector.GreaterThan(normalLength, new Vector<float>(1e-10f)), Vector<float>.One / normalLength, Vector<float>.Zero), out var normal);

            Vector3Wide.CrossWithoutOverlap(ac, normal, out projection.JacobianB);
            Vector3Wide.CrossWithoutOverlap(normal, ab, out projection.JacobianC);
            // 与体积约束类似,我们可以为JacobianA创建一个类似的表达式,但是只做几次加法是很便宜的。
            Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA);

            // 我们可以存储：
            // 雅各比人(2*3)
            // 有效质量(1)
            // 反惯量(1*3,因为我们不需要角惯量)
            // 因为我们不需要惯性张量,这比预乘变量要好。

            Vector3Wide.Dot(negatedJacobianA, negatedJacobianA, out var contributionA);
            Vector3Wide.Dot(projection.JacobianB, projection.JacobianB, out var contributionB);
            Vector3Wide.Dot(projection.JacobianC, projection.JacobianC, out var contributionC);

            // 通过填充雅可比贡献来防止奇异性。这在很大程度上是一种黑客行为,但也是一种相当简单的黑客行为。
            // 与尝试保护InverseEffectiveMass值本身相比,它对调整的敏感度较低,因为它对比例和质量都很敏感。

            // 根据目标区域选择ε。请注意,面积~=宽度^2和我们的雅可比贡献类似于(ac×N)*(Ac X N)。
            // 给定N垂直于AC,||(Ac X N)||==||Ac|,所以贡献就是||Ac|^2。给定平方,它与面积成正比,面积是一个合适的ε源。
            var epsilon = 5e-4f * prestep.TargetScaledArea;
            contributionA = Vector.Max(epsilon, contributionA);
            contributionB = Vector.Max(epsilon, contributionB);
            contributionC = Vector.Max(epsilon, contributionC);
            var inverseEffectiveMass = contributionA * inertiaA.InverseMass + contributionB * inertiaB.InverseMass + contributionC * inertiaC.InverseMass;
            projection.InverseMassA = inertiaA.InverseMass;
            projection.InverseMassB = inertiaB.InverseMass;
            projection.InverseMassC = inertiaC.InverseMass;

            SpringSettingsWide.ComputeSpringiness(prestep.SpringSettings, dt, out var positionErrorToVelocity, out var effectiveMassCFMScale, out projection.SoftnessImpulseScale);

            projection.EffectiveMass = effectiveMassCFMScale / inverseEffectiveMass;
            // 计算位置误差和偏置速度。计算误差时请注意减法的顺序-我们希望偏置速度抵消分离。
            projection.BiasImpulse = (prestep.TargetScaledArea - normalLength) * (1f / 2f) * positionErrorToVelocity * projection.EffectiveMass;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static void ApplyImpulse(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC,
            ref AreaConstraintProjection projection, ref Vector3Wide negatedJacobianA, ref Vector<float> impulse)
        {
            Vector3Wide.Scale(negatedJacobianA, projection.InverseMassA * impulse, out var negativeVelocityChangeA);
            Vector3Wide.Scale(projection.JacobianB, projection.InverseMassB * impulse, out var velocityChangeB);
            Vector3Wide.Scale(projection.JacobianC, projection.InverseMassC * impulse, out var velocityChangeC);
            Vector3Wide.Subtract(velocityA.Linear, negativeVelocityChangeA, out velocityA.Linear);
            Vector3Wide.Add(velocityB.Linear, velocityChangeB, out velocityB.Linear);
            Vector3Wide.Add(velocityC.Linear, velocityChangeC, out velocityC.Linear);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void WarmStart(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref AreaConstraintProjection projection, ref Vector<float> accumulatedImpulse)
        {
            Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA);
            ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref accumulatedImpulse);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Solve(ref BodyVelocities velocityA, ref BodyVelocities velocityB, ref BodyVelocities velocityC, ref AreaConstraintProjection projection, ref Vector<float> accumulatedImpulse)
        {
            // CSI=投射.BiasImPulse-累加脉冲*投射.SoftnessImPulseScale-(csia线性+csiaAngular+csib线性+csibAngular);
            Vector3Wide.Add(projection.JacobianB, projection.JacobianC, out var negatedJacobianA);
            Vector3Wide.Dot(negatedJacobianA, velocityA.Linear, out var negatedContributionA);
            Vector3Wide.Dot(projection.JacobianB, velocityB.Linear, out var contributionB);
            Vector3Wide.Dot(projection.JacobianC, velocityC.Linear, out var contributionC);
            var csv = contributionB + contributionC - negatedContributionA;
            var csi = projection.BiasImpulse - accumulatedImpulse * projection.SoftnessImpulseScale - csv * projection.EffectiveMass;
            accumulatedImpulse += csi;

            ApplyImpulse(ref velocityA, ref velocityB, ref velocityC, ref projection, ref negatedJacobianA, ref csi);
        }
    }


    /// <summary>
    /// 处理一组球窝约束的求解迭代。
    /// </summary>
    public class AreaConstraintTypeProcessor : ThreeBodyTypeProcessor<AreaConstraintPrestepData, AreaConstraintProjection, Vector<float>, AreaConstraintFunctions>
    {
        public const int BatchTypeId = 36;
    }
}
